#include "digitalfiltering.h"
#include <cmath>

DigitalFiltering::DigitalFiltering()
{
}

/*************************************************
**Function:        fft()
**Description:     快速傅里叶变换
**Input:           Dir: 0:正变换, != 0: 反变换
**                 m: 2^m 个点
**                 real: 实部
**                 imag: 虚部
**Return:          无
**Others:          参考自 http://paulbourke.net/miscellaneous//dft/
*************************************************/
void DigitalFiltering::fft(int dir, int m, double *real, double *imag)
{
    long n,i,i1,j,k,i2,l,l1,l2;
    double c1,c2,tx,ty,t1,t2,u1,u2,z;

    // 计算总点数
    n = 1 << m;

    // 位反转
    i2 = n >> 1;
    j = 0;
    for (i=0;i<n-1;i++) {
        if (i < j) {
            tx = real[i];
            ty = imag[i];
            real[i] = real[j];
            imag[i] = imag[j];
            real[j] = tx;
            imag[j] = ty;
        }
        k = i2;
        while (k <= j) {
            j -= k;
            k >>= 1;
        }
        j += k;
    }

    // 变换
    c1 = -1.0;
    c2 = 0.0;
    l2 = 1;
    for (l=0;l<m;l++) {
        l1 = l2;
        l2 <<= 1;
        u1 = 1.0;
        u2 = 0.0;
        for (j=0;j < l1;j++) {
            for (i=j;i < n;i += l2) {
                i1 = i + l1;
                t1 = u1 * real[i1] - u2 * imag[i1];
                t2 = u1 * imag[i1] + u2 * real[i1];
                real[i1] = real[i] - t1;
                imag[i1] = imag[i] - t2;
                real[i] += t1;
                imag[i] += t2;
            }
            z =  u1 * c1 - u2 * c2;
            u2 = u1 * c2 + u2 * c1;
            u1 = z;
        }
        c2 = sqrt((1.0 - c1) / 2.0);
        if (dir) {
            c2 = -c2;
        }
        c1 = sqrt((1.0 + c1) / 2.0);
    }

    // 逆变换时进行缩放
    if (dir) {
        for (i=0;i < n;i++) {
            real[i] /= n;
            imag[i] /= n;
        }
    }
}

/*************************************************
**Function:        fftshift()
**Description:     频谱搬移
**Input:           data: 搬移数据
**                 len: 数据总量
**Return:          无
**Others:          参考自 http://paulbourke.net/miscellaneous//dft/
*************************************************/
void DigitalFiltering::fftshift(double *data, int len)
{
    for (int i = 0;i < (len / 2);i++) {
        double r1 = data[i];
        double r2 = data[len/2 + i];

        data[i] = r2;
        data[len / 2 + i] = r1;
    }
}
/*************************************************
**Function:        fftWithShift()
**Description:     FFT计算（任意长度）
**Input:           signal: 时域信号
**                 resultBits: 数据总量位数
**                 scaleByLen：归一化
**Return:          频域信号
**Others:          对fft的再封装，可输入任意长度时域信号
**                 对不满足2^m信号长度的信号进行裁剪/补0
*************************************************/
QVector<double> DigitalFiltering::fftWithShift(QVector<double> &signal, int resultBits, bool scaleByLen)
{
    QVector<double> result;
    int taps = signal.size();
    int resultLen = 1 << resultBits;

    double *signal_vector = new double[resultLen];
    double *imag = new double[resultLen];

    if (resultLen < taps) {
        int sizeDiffHalf = (taps - resultLen) / 2;
        signal.remove(0, sizeDiffHalf);
        signal.resize(resultLen);

        for(int i = 0;i < resultLen;i++) {
            signal_vector[i] = signal[i];
            imag[i] = 0;
        }
    } else {
        int sizeDiffHalf = (resultLen - taps) / 2;
        for(int i = 0;i < resultLen;i++) {
            if (i < sizeDiffHalf) {
                signal_vector[i] = 0;
            } else if (i < (taps + sizeDiffHalf)) {
                signal_vector[i] = signal[i - sizeDiffHalf];
            } else {
                signal_vector[i] = 0;
            }
            imag[i] = 0;
        }
    }

    fftshift(signal_vector, resultLen);
    fft(0, resultBits, signal_vector, imag);

    double div_factor = scaleByLen ? (double)taps : 1.0;
    for(int i = 0;i < resultLen;i++) {
        result.append(fabs(signal_vector[i]) / div_factor);
    }

    delete[] signal_vector;
    delete[] imag;

    return result;
}
